#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h> 
#include <sys/socket.h>
#include <netinet/in.h>
#include <libplayerinterface/player.h>

#include <core/gui_server.h>

void error(const char *msg) {
  perror(msg);
  exit(1);
}

GUIServer::GUIServer(SyncedCurrentPose *synced_current_pose,
                     SyncedCurrentMission *synced_current_mission) :
  synced_current_pose_(synced_current_pose),
  synced_current_mission_(synced_current_mission) {};

void GUIServer::operator()() {
  printf("I'm alive!!!");
  const int BUFFER_SIZE = 256;
  double px, py, pa;
  int sockfd, newsockfd;
  int portno = 1999;
  socklen_t clilen;
  char buffer[BUFFER_SIZE];
  char reply_buffer[BUFFER_SIZE];
  struct sockaddr_in serv_addr, cli_addr;
  int n;
  sockfd = socket(AF_INET, SOCK_STREAM, 0);
  if (sockfd < 0) 
    error("ERROR opening socket");
  bzero((char *) &serv_addr, sizeof(serv_addr));
  serv_addr.sin_family = AF_INET;
  serv_addr.sin_addr.s_addr = INADDR_ANY;
  serv_addr.sin_port = htons(portno);
  if (bind(sockfd, 
      (struct sockaddr *) &serv_addr,
      sizeof(serv_addr)) < 0) 
    error("ERROR on binding");
  listen(sockfd, 5);
  clilen = sizeof(cli_addr);
  while (1) {
    newsockfd = accept(sockfd, 
      (struct sockaddr *) &cli_addr, 
      &clilen);
    if (newsockfd < 0) 
      error("ERROR on accept");
    bzero(buffer, 256);
    n = read(newsockfd,buffer,255);
    if (n < 0) error("ERROR reading from socket");
    
    // Process request.
    //printf("Here is the message: %s\n", buffer);
    if (strcmp(buffer, "pose?") == 0) {
      player_pose2d pose = synced_current_pose_->current_pose();
      px = pose.px;
      py = pose.py;
      pa = pose.pa;
      // Reply pose to client.
      sprintf(reply_buffer, "%.2lf %.2lf %.2lf", px, py, pa);
    } else {
      // Check if robot is in transit.
      if (!synced_current_mission_->accomplished()) {
    	// reply "Ocupado atendendo missão".
    	strcpy(reply_buffer, "Ocupado atendendo missão.");
      } else {
        sscanf(buffer, "%lf %lf %lf", &px, &py, &pa);  // Get pose from client.
        player_pose2d mission;
        mission.px = px;
        mission.py = py;
        mission.pa = pa;
        printf("parsed: %.2lf %.2lf %.2lf\n", mission.px,
        									  mission.py,
        									  mission.pa);
        synced_current_mission_->set_current_mission(mission);
        strcpy(reply_buffer, "É para já!");  // Reply success.
      }
    }
    
    // Respond.
    n = write(newsockfd, reply_buffer, strlen(reply_buffer));
    if (n < 0) error("ERROR writing to socket");
    close(newsockfd);
  }
  close(sockfd);
}
